//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#include <buola/image/algorithm.h>
#include <buola/image/cimageplugin.h>
#include <buola/image/canyimage.h>
#include <buola/image/io.h>
#include <buola/algorithm/split.h>
#include <buola/mat/cquat.h>
#include <buola/cv/ccloud.h>
#include <buola/zz/zreal.h>
#include <buola/zz/zextreal.h>
#include <buola/zz/zchar.h>
#include <buola/zz/operators.h>
#include <buola/zz/repeat.h>
#include <buola/io/cbinaryreader.h>

namespace {

using namespace buola;
    
enum EDataType
{
    DATA_ASCII,
    DATA_BINARY,
    DATA_BINARY_COMPRESSED
};

struct CPCDField
{
    explicit CPCDField(const std::string &pField)
        :   mField(pField)
        ,   mSize(4)
        ,   mType('F')
        ,   mCount(1)
    {}

    std::string mField;
    size_t mSize;
    char mType;
    size_t mCount;
};

struct CPCDHeader
{
    std::vector<CPCDField> mFields;
    CSize_i mSize;
    mat::CVec3f mOrigin;
    mat::CQuat_f mRotation;
    EDataType mDataType;
};

static ptrdiff_t get_rgb_pos(const std::vector<CPCDField> &pFields)
{
    ptrdiff_t lPos=0;
    for(const auto &f : pFields)
    {
        if(f.mField=="rgb")
            return lPos;
        else if(f.mField=="rgba")
            return lPos;
        lPos+=f.mSize*f.mCount;
    }
    return -1;
}

static ptrdiff_t get_x_pos(const std::vector<CPCDField> &pFields)
{
    ptrdiff_t lPos=0;
    for(const auto &f : pFields)
    {
        if(f.mField=="x")
            return lPos;
        lPos+=f.mSize*f.mCount;
    }
    return -1;
}

static ptrdiff_t get_y_pos(const std::vector<CPCDField> &pFields)
{
    ptrdiff_t lPos=0;
    for(const auto &f : pFields)
    {
        if(f.mField=="y")
            return lPos;
        lPos+=f.mSize*f.mCount;
    }
    return -1;
}

static ptrdiff_t get_z_pos(const std::vector<CPCDField> &pFields)
{
    ptrdiff_t lPos=0;
    for(const auto &f : pFields)
    {
        if(f.mField=="z")
            return lPos;
        lPos+=f.mSize*f.mCount;
    }
    return -1;
}

static void extract_data_binary(io::PBufferStream pSrc,std::size_t pSize,uint8_t *pData)
{
    if(pSrc->Read(pData,pSize).Count()!=pSize)
        throw XInvalid("can't read PCD data");
}

static void extract_data_binary_compressed(io::PBufferStream pSrc,std::size_t pSize,uint8_t *pData)
{
    uint32_t lCSize,lUSize;
    io::CBinaryReader lBin(pSrc);
    lBin >> lCSize >> lUSize;
    if(lUSize!=pSize)
        throw XInvalid("can't read compressed PCD data: invalid stored size");

    std::unique_ptr<uint8_t[]> lCData(new uint8_t[lCSize]);
    if(pSrc->Read(lCData.get(),lCSize).Count()!=lCSize)
        throw XInvalid("can't read PCD data");

    const uint8_t *lIn=lCData.get();
    const uint8_t *lInEnd=lIn+lCSize;
    uint8_t *lOut=pData;
    uint8_t *lOutEnd=pData+pSize;

    while(lIn<lInEnd)
    {
        uint32_t lCtrl=*lIn++;

        if (lCtrl<(1<<5)) //literal run
        {
            lCtrl++;

            if(lOut+lCtrl>lOutEnd||lIn+lCtrl>lInEnd)
                throw XInvalid("can't read compressed PCD data: overflow");

            for(int i=0;i<lCtrl;i++)
                *lOut++=*lIn++;
        }
        else //back reference
        {
            uint32_t lLen=lCtrl>>5;

            uint8_t *lRef=lOut-((lCtrl&0x1f)<<8)-1;

            if(lIn>=lInEnd)
                throw XInvalid("can't read compressed PCD data: underflow");

            if(lLen==7)
            {
                lLen+=*lIn++;
                if(lIn>=lInEnd)
                    throw XInvalid("can't read compressed PCD data: wrong length");
            }
            lRef-=*lIn++;
            lLen+=2;
            if(lOut+lLen>lOutEnd||lRef<pData)
                throw XInvalid("can't read compressed PCD data: eof");

            for(int i=0;i<lLen;i++)
                *lOut++=*lRef++;
        }
    }

    if(lOut!=lOutEnd)
    {
        throw XInvalid("can't read compressed PCD data: eof");
    }
}

static CPCDHeader read_pcd_header(io::PBufferStream pSrc)
{
    CPCDHeader lHeader;
    lHeader.mSize={0,1};

    std::string lString;

    int lStep=0;

    while(io::get_line(pSrc,lString))
    {
        if(lString.empty()||lString[0]=='#') continue;

        std::vector<std::string> lSplit;
        split(lString,lSplit,fn::is_ascii_space(),nDeleteEmpty);

        for(int i=0;i<lSplit.size();i++)
        {
            if(lSplit[i][0]=='#')
            {
                lSplit.resize(i);
                break;
            }
        }

        if(lSplit.empty()) continue;

        switch(lStep)
        {
        case 0:
            if(lSplit[0]=="FIELDS")
            {
                if(lSplit.size()<2)
                    throw XInvalid("invalid PCD file: no fields");
                for(int i=1;i<lSplit.size();i++)
                    lHeader.mFields.emplace_back(lSplit[i]);
                lStep=1;
                break;
            }
        case 1:
            if(lSplit[0]=="SIZE")
            {
                if(lHeader.mFields.empty())
                    throw XInvalid("invalid PCD file: no fields");
                if(lSplit.size()!=1+lHeader.mFields.size())
                    throw XInvalid("invalid PCD file: wrong number of size elements");
                for(int i=1;i<lSplit.size();i++)
                    lHeader.mFields[i-1].mSize=to_<size_t>(lSplit[i]);
                lStep=2;
                break;
            }
        case 2:
            if(lSplit[0]=="TYPE")
            {
                if(lHeader.mFields.empty())
                    throw XInvalid("invalid PCD file: no fields");
                if(lSplit.size()!=1+lHeader.mFields.size())
                    throw XInvalid("invalid PCD file: wrong number of type elements");
                for(int i=1;i<lSplit.size();i++)
                {
                    if(lSplit[i].size()!=1||(lSplit[i][0]!='I'&&lSplit[i][0]!='U'&&lSplit[i][0]!='F'))
                        throw XInvalid("invalid PCF file: invalid data type");
                    lHeader.mFields[i-1].mType=lSplit[i][0];
                }
                lStep=3;
                break;
            }
        case 3:
            if(lSplit[0]=="COUNT")
            {
                if(lHeader.mFields.empty())
                    throw XInvalid("invalid PCD file: no fields");
                if(lSplit.size()!=1+lHeader.mFields.size())
                    throw XInvalid("invalid PCD file: wrong number of count elements");
                for(int i=1;i<lSplit.size();i++)
                    lHeader.mFields[i-1].mCount=to_<size_t>(lSplit[i]);
                lStep=4;
                break;
            }
        case 4:
            if(lSplit[0]=="WIDTH")
            {
                if(lSplit.size()!=2)
                    throw XInvalid("invalid PCD file: wrong WIDTH element");
                lHeader.mSize.x=to_<int>(lSplit[1]);
                lStep=5;
                break;
            }
        case 5:
            if(lSplit[0]=="HEIGHT")
            {
                if(lSplit.size()!=2)
                    throw XInvalid("invalid PCD file: wrong HEIGHT element");
                lHeader.mSize.y=to_<int>(lSplit[1]);
                lStep=6;
                break;
            }
        case 6:
            if(lSplit[0]=="VIEWPOINT")
            {
                if(lSplit.size()!=8)
                    throw XInvalid("invalid PCD file: wrong VIEWPOINT element");
                lHeader.mOrigin={to_<float>(lSplit[1]),to_<float>(lSplit[2]),to_<float>(lSplit[3])};
                lHeader.mRotation={to_<float>(lSplit[4]),to_<float>(lSplit[5]),to_<float>(lSplit[6]),to_<float>(lSplit[7])};
                lStep=7;
                break;
            }
        case 7:
            if(lSplit[0]=="DATA")
            {
                if(lHeader.mFields.empty())
                    throw XInvalid("invalid PCD file: no fields");
                if(lSplit.size()!=2)
                    throw XInvalid("invalid PCD file: wrong DATA element");

                if(lSplit[1]=="ascii")
                    lHeader.mDataType=DATA_ASCII;
                else if(lSplit[1]=="binary")
                    lHeader.mDataType=DATA_BINARY;
                else if(lSplit[1]=="binary_compressed")
                    lHeader.mDataType=DATA_BINARY_COMPRESSED;
                else
                    throw XInvalid("invalid PCD file: invalid DATA type");

                return lHeader;
            }
        }
    }

    throw XInvalid("invalid PCD file: unexpected end of file");
}

static void write_pcd_header(io::PBufferStream pDst,const CPCDHeader &pHeader)
{
    io::CTextWriter lWriter(pDst);
    lWriter << "# PCD Point Cloud\n";
    lWriter << "VERSION 0.7\n";
    lWriter << "FIELDS";
    for(const auto &f : pHeader.mFields)
        lWriter << ' ' << f.mField;
    lWriter << "\nSIZE";
    for(const auto &f : pHeader.mFields)
        lWriter << ' ' << f.mSize;
    lWriter << "\nTYPE";
    for(const auto &f : pHeader.mFields)
        lWriter << ' ' << f.mType;
    lWriter << "\nCOUNT";
    for(const auto &f : pHeader.mFields)
        lWriter << ' ' << f.mCount;
    lWriter << "\nWIDTH " << pHeader.mSize.x << "\n";
    lWriter << "HEIGHT " << pHeader.mSize.y << "\n";
    lWriter << "VIEWPOINT " << pHeader.mOrigin.x() << ' ' << pHeader.mOrigin.y() << ' ' << pHeader. mOrigin.z() << ' ' <<
            pHeader.mRotation.w << ' ' << pHeader.mRotation.x << ' ' << pHeader.mRotation.y << ' ' << pHeader.mRotation.z << "\n";
    lWriter << "POINTS " << pHeader.mSize.x*pHeader.mSize.y << "\n";
    lWriter << "DATA binary\n";
}

/*namespace*/ }

namespace buola { namespace img {

class CPCDCodec : public CCodec
{
public:
    virtual ~CPCDCodec() {}

    void Read(io::PBufferStream pSrc,CAnyImage &pDst,EImageFormat pFmt,const CSize_i &pSize);
    void Write(const CAnyView &pSrc,io::PBufferStream pDst);
};

class CPCDPlugin : public CImagePlugin
{
public:
    CPCDPlugin()
    {
        RegisterPlugin("pcd",this);
        RegisterPlugin("PCD",this);
    }

    CCodec *GetInstance()
    {
        return new CPCDCodec;
    }
};

static CPCDPlugin sPCDPlugin;

void CPCDCodec::Read(io::PBufferStream pSrc,CAnyImage &pDst,EImageFormat pFmt,const CSize_i& /*pSize*/)
{
    CPCDHeader lHeader=read_pcd_header(pSrc);

    size_t lPointSize=0;
    for(const auto &f : lHeader.mFields)
        lPointSize+=f.mSize*f.mCount;

    if(lHeader.mDataType==DATA_ASCII)
    {
        if(pFmt==FORMAT_RGB8)
        {
            img::CImage_rgb8 lImage(lHeader.mSize);
            uint8_t *lDst=lImage.RawData();

            std::string lStr;
            while(io::get_line(pSrc,lStr))
            {
                std::array<float,4> lData;
                if(!zz::phrase_parse(lStr.begin(),lStr.end(),zz::repeat<4>(zz::ext_flt()),zz::chr(' '),lData)) 
                    throw XInvalid("can't read PCD file");

                const uint8_t *lRGB=reinterpret_cast<const uint8_t*>(&lData[3]);
                *lDst++=lRGB[2];
                *lDst++=lRGB[1];
                *lDst++=lRGB[0];
            }
            
            pDst=std::move(lImage);
        }
        else if(pFmt==FORMAT_POINT_RGB8)
        {
            cvi::CImage_point_rgb8 lImage(lHeader.mSize);
            auto *lDst=lImage.RawData();

            std::string lStr;
            while(io::get_line(pSrc,lStr))
            {
                std::array<float,4> lData;
                if(!zz::phrase_parse(lStr.begin(),lStr.end(),zz::repeat<4>(zz::ext_flt()),zz::chr(' '),lData)) 
                    throw XInvalid("can't read PCD file");

                lDst->mPoint.x()=lData[0];
                lDst->mPoint.y()=lData[1];
                lDst->mPoint.z()=lData[2];
                const uint8_t *lRGB=reinterpret_cast<const uint8_t*>(&lData[3]);
                lDst->mPixel[0]=lRGB[2];
                lDst->mPixel[1]=lRGB[1];
                lDst->mPixel[2]=lRGB[0];
                
                ++lDst;
            }
            
            pDst=std::move(lImage);
        }
        else
        {
            throw XInvalid("invalid format reading PCD file");
        }
    }
    else
    {
        size_t lPoints=lHeader.mSize.x*lHeader.mSize.y;
        size_t lTotalSize=lPointSize*lPoints;

        std::unique_ptr<uint8_t[]> lDataArray(new uint8_t[lTotalSize]);
        uint8_t *lData=lDataArray.get();

        if(lHeader.mDataType==DATA_BINARY)
            extract_data_binary(pSrc,lTotalSize,lData);
        else
            extract_data_binary_compressed(pSrc,lTotalSize,lData);

        if(pFmt==FORMAT_RGB8)
        {
            img::CImage_rgb8 lImage(lHeader.mSize);
            uint8_t *lDst=lImage.RawData();

            ptrdiff_t lPos=get_rgb_pos(lHeader.mFields);

            if(lPos==-1)
                throw XInvalid("can't read PCD file as image: no RGB info");

            uint8_t *lSrc;
            ptrdiff_t lInc;

            if(lHeader.mDataType==DATA_BINARY_COMPRESSED)
            {
                lSrc=lData+lPos*lPoints;
                lInc=4;
            }
            else
            {
                lSrc=lData+lPos;
                lInc=lPointSize;
            }

            for(int i=0;i<lPoints;i++)
            {
                *lDst++=lSrc[2];
                *lDst++=lSrc[1];
                *lDst++=lSrc[0];
                lSrc+=lInc;
            }

            pDst=std::move(lImage);
        }
        else if(pFmt==FORMAT_POINT_RGB8)
        {
            cvi::CImage_point_rgb8 lImage(lHeader.mSize);
            auto *lDst=lImage.RawData();

            ptrdiff_t lRGBPos=get_rgb_pos(lHeader.mFields);
            ptrdiff_t lXPos=get_x_pos(lHeader.mFields);
            ptrdiff_t lYPos=get_y_pos(lHeader.mFields);
            ptrdiff_t lZPos=get_z_pos(lHeader.mFields);

            if(lXPos==-1||lYPos==-1||lZPos==-1||lRGBPos==-1)
                throw XInvalid("can't read PCD file as image: no RGB info");

            uint8_t *lX,*lY,*lZ,*lRGB;
            ptrdiff_t lXInc,lYInc,lZInc,lRGBInc;
        
            if(lHeader.mDataType==DATA_BINARY_COMPRESSED)
            {
                lX=lData+lXPos*lPoints;
                lY=lData+lYPos*lPoints;
                lZ=lData+lZPos*lPoints;
                lRGB=lData+lRGBPos*lPoints;

                lXInc=4;
                lYInc=4;
                lZInc=4;
                lRGBInc=4;
            }
            else
            {
                lX=lData+lXPos;
                lY=lData+lYPos;
                lZ=lData+lZPos;
                lRGB=lData+lRGBPos;

                lXInc=lPointSize;
                lYInc=lPointSize;
                lZInc=lPointSize;
                lRGBInc=lPointSize;
            }

            for(int i=0;i<lPoints;i++)
            {
                lDst->mPixel[0]=lRGB[2];
                lDst->mPixel[1]=lRGB[1];
                lDst->mPixel[2]=lRGB[0];
                lDst->mPoint.x()=*reinterpret_cast<const float*>(lX);
                lDst->mPoint.y()=*reinterpret_cast<const float*>(lY);
                lDst->mPoint.z()=*reinterpret_cast<const float*>(lZ);

                lX+=lXInc;
                lY+=lYInc;
                lZ+=lZInc;
                lRGB+=lRGBInc;
                lDst++;
            }

            pDst=std::move(lImage);
        }
        else
        {
            throw XInvalid("invalid format reading PCD file");
        }
    }
}

void CPCDCodec::Write(const img::CAnyView &pSrc,io::PBufferStream pDst)
{
    CPCDHeader lHeader;
    
    cvi::CView_point_rgb8 lSrc=pSrc.View<cvi::TFormat_point_rgb8>();
    
    lHeader.mSize=lSrc.Size();
    lHeader.mOrigin={0,0,0};
    lHeader.mRotation=mat::CQuat_f();
    lHeader.mDataType=DATA_BINARY;
    lHeader.mFields.emplace_back("x");
    lHeader.mFields.emplace_back("y");
    lHeader.mFields.emplace_back("z");
    lHeader.mFields.emplace_back("rgb");
    
    write_pcd_header(pDst,lHeader);
    
    size_t lPoints=lHeader.mSize.x*lHeader.mSize.y;

    std::unique_ptr<uint8_t[]> lDataArray(new uint8_t[lPoints*4*sizeof(float)]);
    uint8_t *lData=lDataArray.get();
    
    auto *lSrcPtr=lSrc.RawData();

    for(int i=0;i<lPoints;i++)
    {
        *reinterpret_cast<float*>(lData)=lSrcPtr->mPoint.x();
        *reinterpret_cast<float*>(lData+4)=lSrcPtr->mPoint.y();
        *reinterpret_cast<float*>(lData+8)=lSrcPtr->mPoint.z();
        lData[12]=lSrcPtr->mPixel[2];
        lData[13]=lSrcPtr->mPixel[1];
        lData[14]=lSrcPtr->mPixel[0];
        lData[15]=255;
        lData+=16;
        ++lSrcPtr;
    }
    
    pDst->Write(lDataArray.get(),lPoints*4*sizeof(float));
}

void load_rgbd(CImage_rgb8 &pRGB,CImage_gray32f &pDepth,const io::CURI &pURI)
{
    io::PBufferStream lSrc=io::open_buffered(pURI,io::EMode::READ);
    CPCDHeader lHeader=read_pcd_header(lSrc);

    size_t lPointSize=0;
    for(const auto &f : lHeader.mFields)
        lPointSize+=f.mSize*f.mCount;

    pRGB.Recreate(lHeader.mSize);
    pDepth.Recreate(lHeader.mSize);
    uint8_t *lDstRGB=pRGB.RawData();
    float *lDstDepth=pDepth.RawData();

    if(lHeader.mDataType==DATA_ASCII)
    {
        std::string lStr;
        while(get_line(lSrc,lStr))
        {
            std::array<float,4> lData;
            if(!zz::phrase_parse(lStr.begin(),lStr.end(),zz::repeat<4>(zz::ext_flt()),zz::chr(' '),lData)) 
                throw XInvalid("can't read PCD file");

            *lDstDepth++=lData[2];
            const uint8_t *lRGB=reinterpret_cast<const uint8_t*>(&lData[3]);
            *lDstRGB++=lRGB[2];
            *lDstRGB++=lRGB[1];
            *lDstRGB++=lRGB[0];
        }
    }
    else
    {
        size_t lPoints=lHeader.mSize.x*lHeader.mSize.y;
        size_t lTotalSize=lPointSize*lPoints;

        std::unique_ptr<uint8_t[]> lDataArray(new uint8_t[lTotalSize]);
        uint8_t *lData=lDataArray.get();

        if(lHeader.mDataType==DATA_BINARY)
            extract_data_binary(lSrc,lTotalSize,lData);
        else
            extract_data_binary_compressed(lSrc,lTotalSize,lData);

        ptrdiff_t lRGBPos=get_rgb_pos(lHeader.mFields);
        ptrdiff_t lZPos=get_z_pos(lHeader.mFields);

        if(lZPos==-1||lRGBPos==-1)
            throw XInvalid("can't read PCD file as image: no RGB info");

        uint8_t *lZ,*lRGB;
        ptrdiff_t lZInc,lRGBInc;
        
        if(lHeader.mDataType==DATA_BINARY_COMPRESSED)
        {
            lZ=lData+lZPos*lPoints;
            lRGB=lData+lRGBPos*lPoints;

            lZInc=4;
            lRGBInc=4;
        }
        else
        {
            lZ=lData+lZPos;
            lRGB=lData+lRGBPos;

            lZInc=lPointSize;
            lRGBInc=lPointSize;
        }

        for(int i=0;i<lPoints;i++)
        {
            *lDstRGB++=lRGB[2];
            *lDstRGB++=lRGB[1];
            *lDstRGB++=lRGB[0];
            *lDstDepth++=*reinterpret_cast<const float*>(lZ);
            
            lZ+=lZInc;
            lRGB+=lRGBInc;
        }
    }
}

/*namespace img*/ } /*namespace buola*/ }

namespace buola { namespace cvi {

CCloud_rgb8 load_cloud_pcd(const io::CURI &pURI)
{
    io::PBufferStream lF=io::open_buffered(pURI,io::EMode::READ);

    CPCDHeader lHeader=read_pcd_header(lF);

    size_t lPointSize=0;
    for(const auto &f : lHeader.mFields)
        lPointSize+=f.mSize*f.mCount;

    CCloud_rgb8 lCloud(lHeader.mOrigin,lHeader.mRotation);

    if(lHeader.mDataType==DATA_ASCII)
    {
        std::string lStr;
        while(get_line(lF,lStr))
        {
            std::array<float,4> lData;
            if(!zz::phrase_parse(lStr.begin(),lStr.end(),zz::repeat<4>(zz::dbl()),zz::chr(' '),lData)) continue;

            CCloud_rgb8::TPoint lPoint;

            lPoint.mPoint.x()=lData[0];
            lPoint.mPoint.y()=lData[1];
            lPoint.mPoint.z()=lData[2];
            const uint8_t *lRGB=reinterpret_cast<const uint8_t*>(&lData[3]);
            lPoint.mPixel[0]=lRGB[2];
            lPoint.mPixel[1]=lRGB[1];
            lPoint.mPixel[2]=lRGB[0];

            lCloud.push_back(lPoint);
        }
    }
    else
    {
        size_t lPoints=lHeader.mSize.x*lHeader.mSize.y;
        size_t lTotalSize=lPointSize*lPoints;

        std::unique_ptr<uint8_t[]> lDataArray(new uint8_t[lTotalSize]);
        uint8_t *lData=lDataArray.get();

        if(lHeader.mDataType==DATA_BINARY)
            extract_data_binary(lF,lTotalSize,lData);
        else
            extract_data_binary_compressed(lF,lTotalSize,lData);

        ptrdiff_t lRGBPos=get_rgb_pos(lHeader.mFields);
        ptrdiff_t lXPos=get_x_pos(lHeader.mFields);
        ptrdiff_t lYPos=get_y_pos(lHeader.mFields);
        ptrdiff_t lZPos=get_z_pos(lHeader.mFields);

        if(lXPos==-1||lYPos==-1||lZPos==-1||lRGBPos==-1)
            throw XInvalid("can't read PCD file as image: missing fields");

        uint8_t *lX,*lY,*lZ,*lRGB;
        ptrdiff_t lXInc,lYInc,lZInc,lRGBInc;
        
        if(lHeader.mDataType==DATA_BINARY_COMPRESSED)
        {
            lX=lData+lXPos*lPoints;
            lY=lData+lYPos*lPoints;
            lZ=lData+lZPos*lPoints;
            lRGB=lData+lRGBPos*lPoints;

            lXInc=4;
            lYInc=4;
            lZInc=4;
            lRGBInc=4;
        }
        else
        {
            lX=lData+lXPos;
            lY=lData+lYPos;
            lZ=lData+lZPos;
            lRGB=lData+lRGBPos;

            lXInc=lPointSize;
            lYInc=lPointSize;
            lZInc=lPointSize;
            lRGBInc=lPointSize;
        }

        for(int i=0;i<lPoints;i++)
        {
            CCloud_rgb8::TPoint lPoint;

            lPoint.mPoint.x()=*reinterpret_cast<const float*>(lX);
            if(!is_nan(lPoint.mPoint.x()))
            {
                lPoint.mPoint.y()=*reinterpret_cast<const float*>(lY);
                lPoint.mPoint.z()=*reinterpret_cast<const float*>(lZ);

                lPoint.mPixel[0]=lRGB[2];
                lPoint.mPixel[1]=lRGB[1];
                lPoint.mPixel[2]=lRGB[0];

                lCloud.push_back(lPoint);
            }

            lX+=lXInc;
            lY+=lYInc;
            lZ+=lZInc;
            lRGB+=lRGBInc;
        }
    }

    return lCloud;
}
    
void save_cloud_pcd(const CCloud_rgb8 &pCloud,const io::CURI &pURI)
{
    io::PBufferStream lF=io::open_buffered(pURI,io::EMode::WRITE);
    
    CPCDHeader lHeader;
    
    lHeader.mSize=CSize_i(pCloud.size(),1);
    lHeader.mOrigin=pCloud.Origin();
    lHeader.mRotation=pCloud.Rotation();
    lHeader.mDataType=DATA_BINARY;
    lHeader.mFields.emplace_back("x");
    lHeader.mFields.emplace_back("y");
    lHeader.mFields.emplace_back("z");
    lHeader.mFields.emplace_back("rgb");
    
    write_pcd_header(lF,lHeader);
    
    size_t lPoints=pCloud.size();

    std::unique_ptr<uint8_t[]> lDataArray(new uint8_t[lPoints*4*sizeof(float)]);
    uint8_t *lData=lDataArray.get();
    
    for(int i=0;i<lPoints;i++)
    {
        *reinterpret_cast<float*>(lData)=pCloud[i].mPoint.x();
        *reinterpret_cast<float*>(lData+4)=pCloud[i].mPoint.y();
        *reinterpret_cast<float*>(lData+8)=pCloud[i].mPoint.z();
        lData[12]=pCloud[i].mPixel[2];
        lData[13]=pCloud[i].mPixel[1];
        lData[14]=pCloud[i].mPixel[0];
        lData[15]=255;
        lData+=16;
    }
    
    lF->Write(lDataArray.get(),lPoints*4*sizeof(float));
}
    
void save_cloud_pcd(const CCloud_f &pCloud,const io::CURI &pURI)
{
    io::PBufferStream lF=io::open_buffered(pURI,io::EMode::WRITE);
    
    CPCDHeader lHeader;
    
    lHeader.mSize=CSize_i(pCloud.size(),1);
    lHeader.mOrigin=pCloud.Origin();
    lHeader.mRotation=pCloud.Rotation();
    lHeader.mDataType=DATA_BINARY;
    lHeader.mFields.emplace_back("x");
    lHeader.mFields.emplace_back("y");
    lHeader.mFields.emplace_back("z");
    
    write_pcd_header(lF,lHeader);
    
    size_t lPoints=pCloud.size();

    std::unique_ptr<uint8_t[]> lDataArray(new uint8_t[lPoints*3*sizeof(float)]);
    uint8_t *lData=lDataArray.get();
    
    for(int i=0;i<lPoints;i++)
    {
        *reinterpret_cast<float*>(lData)=pCloud[i].mPoint.x();
        *reinterpret_cast<float*>(lData+4)=pCloud[i].mPoint.y();
        *reinterpret_cast<float*>(lData+8)=pCloud[i].mPoint.z();
        lData+=12;
    }
    
    lF->Write(lDataArray.get(),lPoints*3*sizeof(float));
}
    
/*namespace cvi*/ } /*namespace buola*/ }

